﻿#include "Quat.h"
#include "MathBase.h"

#include "Math/Coordinate.h"
#include "Matrix3x3.h"
#include <cassert>

#include "Containers/String.h"

SQuat SQuat::FromEuler(const SVec3& InRadians)
{
	return SQuat(Coordiante::GetUp(),InRadians.Z)*SQuat(Coordiante::GetForward(),InRadians.Y)*SQuat(Coordiante::GetRight(),InRadians.X);
}

SVec3 SQuat::ToEuler() const
{
	// Matrix= cosZ  -sinZ 0            cosY     0     sinY             1     0     0
	// 		   sinZ cosZ 0      *       0        1     0          *     0     cosX  -sinX
	// 		   0     0    1            -sinY    0     cosY             0     sinX  cosX

	// = cZ      -sZ    cZsY         1  0   0
    //   sZcY    cZ     sZsY   *    0  cX  -sX
    //   -sY     0     cY           0  sX  cX

	// =  cZcY      -sZcX+cZsYsX  sZsX+cZsYcX
	// 	 sZcY     cZcX+sZsYsX   -cZsX +sZsYcX
	// 	 -sY       cYsX           cYcX 

	// so, tanX =  M[column 1][row 2]/M[column 2][row 2]  
	//     sinY =  -M[column 0][row 2]
	//     tanZ =  -M[column 0][row 1]  /M[column 0][row 0]
    auto M00 = 1 - 2 * Y * Y - 2 * Z * Z;

    auto M01 = 2 * X * Y + 2 * W * Z;

    auto M02 = 2 * X * Z - 2 * W * Y;
    auto M12 = 2 * Y * Z + 2 * W * X;
    auto M22 = 1 - 2 * X * X - 2 * Y * Y;

	SVec3 Euler;

	Euler.X= CMathBase::Atan(M12/M22);
	Euler.Y= CMathBase::Asin(-M02);
	Euler.Z= CMathBase::Atan(M01/M00);

	return Euler;

}

CString SQuat::ToString() const 
{
	return U"Quat[x:"+CString(X)
            +U",y:"+CString(Y)
            +U",z:"+CString(Z)
            +U",w:"+CString(W)
			+U"]"
			;

}

void SQuat::ToFloatArray(float OutArray[4]) const
{
	OutArray[0] = X;
	OutArray[1] = Y;
	OutArray[2] = Z;
	OutArray[3] = W;
}

// glm::quat SQuat::ToGlm() const
// {
// 	return glm::quat(W,X,Y,Z);
// }

SQuat::SQuat(const SVec3& Axis, Float InRandian)
{
	//w= cos(0.5*Angle)
	//[x,y,z]= sin（0.5*Angle）*Axis

	Float HalfAngle = InRandian *0.5f;
	W = CMathBase::Cos(HalfAngle);

	Float S = CMathBase::Sin(HalfAngle);

	X = S * Axis.X;
	Y = S*  Axis.Y;
	Z = S * Axis.Z;
}


SQuat::SQuat(const SMatrix3x3& Matrix)
{
	Float Trace=Matrix.Data[0][0] +Matrix.Data[1][1] +Matrix.Data[2][2];

	W=0.5 * CMathBase::Sqrt(Trace);

	Float InverseFourW= 1/ (4*W);
	
	X=( Matrix.Data[1][2] - Matrix.Data[2][1] )* InverseFourW;
	Y=( Matrix.Data[2][0] - Matrix.Data[0][2] )* InverseFourW;
	Z=( Matrix.Data[0][1] - Matrix.Data[1][0] )* InverseFourW;

}

SQuat SQuat::operator*(const SQuat& Q) const
{
	SQuat Ret;
	Ret.W = W * Q.W - X * Q.X - Y * Q.Y - Z * Q.Z;
	Ret.X = X * Q.W + W * Q.X - Z * Q.Y + Y * Q.Z;
	Ret.Y = Y * Q.W + Z * Q.X + W * Q.Y - X * Q.Z;
	Ret.Z = Z * Q.W - Y * Q.X + X * Q.Y + W * Q.Z;


	return Ret;
}

SVec3 SQuat::operator*(const SVec3& R) const
{
	//Q=[X,Y,Z]
	//R' = R + 2W(Q x R) +2Q x(Q x R)

	SVec3 Q(X, Y, Z);
	SVec3 QXR = Q.CrossProduct(R) * 2;
	SVec3 Ret = R + W * QXR + Q.CrossProduct(QXR);

	return Ret;
}

SQuat SQuat::operator*(const Float& InScale) const
{
	return SQuat(X*InScale, Y * InScale, Z * InScale, W * InScale);
}

SQuat SQuat::operator/(const Float& InScale) const
{
	return SQuat(X / InScale, Y / InScale, Z / InScale, W / InScale);
}

Float SQuat::DotProduct(const SQuat& Other) const
{
	return X*Other.X+ Y * Other.Y+ Z * Other.Z+ W * Other.W;
}

SQuat SQuat::operator+(const SQuat& Other) const
{
	return SQuat(X+Other.X, Y + Other.Y, Z + Other.Z, W + Other.W);
}

SVec3 SQuat::GetRotationAxis() const
{
	if(CMathBase::Abs(W)==1)
	{
		return  SVec3(0, 0, 1);
	}
	return SVec3(X, Y, Z) / (CMathBase::Sin(CMathBase::Acos(W)));
}

Float SQuat::GetRotationAngle() const
{
	return 2 * CMathBase::Acos(W);
}

SQuat SQuat::Inverse() const
{
	// Q^(-1)=[-X,-Y,-Z,W]  / (Length*Length)
	Float Length = GetLength();
	return SQuat(-X/ Length,-Y/ Length,-Z/ Length,W/ Length);
}

bool SQuat::IsNormalized() const
{
	//TODO  make sure error 
	return CMathBase::Equal(GetLength(),1);
}

void SQuat::Normalize()
{
	(*this)/ GetLength();
}

Float SQuat::GetLength() const
{
	return  CMathBase::Sqrt(X * X + Y * Y + Z * Z + W * W);
}

SVec3 SQuat::GetForwardDirection() const
{
	return SMatrix3x3(*this) * Coordiante::GetForward();
}

SVec3 SQuat::GetRightDirection() const
{
	return SMatrix3x3(*this) * Coordiante::GetRight();
}

SVec3 SQuat::GetUpDirection() const
{
	return SMatrix3x3(*this) * Coordiante::GetUp();
}

SQuat Slerp(const SQuat& A, const SQuat& B, Float Alpha)
{
	// Theta= Acos(DotProduct(A, B))
	// result= sin((1-Alpha)*Theta) /sin(Theta) *A + sin(Alpha*Theta) /sin(Theta) *B

	Float Cos = A.DotProduct(B);

	SQuat NearB = B;
	if (Cos < 0)
	{
		NearB = B * -1;
		Cos=-Cos;
	}

	Float Theta = CMathBase::Acos(Cos);  //[0,PI]

	if (Theta < 0.1)
	{
		//theta is too small ,sin(theta) might =0, cause divide 0 error is use SLerp,
		//use NLerp is fast and accuracey enough
		return Nlerp(A, NearB, Alpha);
	}
	else
	{
		Float Sin = CMathBase::Sin(Theta);
		Float SinA = CMathBase::Sin(Alpha * Theta);
		Float SinOneMinusA = CMathBase::Sin((1 - Alpha) * Theta);
		return (A * SinOneMinusA + NearB * SinA ) / Sin;
	}
}

SQuat Nlerp(const SQuat& A, const SQuat& B, Float Alpha)
{
	SQuat Ret = Lerp(A, B, Alpha);
	Ret.Normalize();
	return Ret;
}

SQuat Lerp(const SQuat& A, const SQuat& B, Float Alpha)
{
	return   A * (1 - Alpha) + B * Alpha;
}
